import trimesh
import os
import numpy as np

def change_ray_color(ray, color):
    colors = np.ones((len(ray.entities), 3))
    colors_1 = (colors * color).astype(np.uint8)  # approach
    ray.colors = colors_1
# root_dir = '/home/lifuyu/hand/DLR_object'
# ori_dir = '/home/lifuyu/hand/BHAM_obj'
# dst = '/home/lifuyu/hand/dlr_obj'
# for root,dirs,files in os.walk(root_dir):
#     for file_name in files:
#         file_path = os.path.join(root,file_name)
#         new_file_name = file_name.replace(".stl",".obj")
#         ori_path = os.path.join(ori_dir,new_file_name)
#         mesh  = trimesh.load_mesh(file_path)
#         ori_mesh = trimesh.load_mesh(ori_path)
#         ori_mesh_box_axis = ori_mesh.bounding_box.extents
#         mesh_box_axis = mesh.bounding_box.extents
#         mesh.vertices *= float(ori_mesh_box_axis[0]/mesh_box_axis[0])
#         mesh.export('{}/{}'.format(dst,file_name))
        # print(ori_mesh_box_axis,mesh_box_axis)
        # exit()

if __name__ =="__main__":
    # file_path = '/home/lifuyu/new_finger.stl'
    file_path = '/home/v-wewei/code/two_stage_pointnet/data/grippers/yumi/coarse/new_finger.stl'
    mesh = trimesh.load_mesh(file_path)
    mesh_box_axis = mesh.bounding_box.extents
    print(mesh_box_axis)
    finger_left = trimesh.load('/home/v-wewei/code/two_stage_pointnet/data/grippers/yumi/'
                               'coarse/box_finger.stl')
    finger_left_axis =finger_left.bounding_box.extents
    print(finger_left_axis)
    # exit()
    rotation = trimesh.transformations.rotation_matrix(
        np.deg2rad(90), [0, 0, 1])
    rotation_ = trimesh.transformations.rotation_matrix(
        np.deg2rad(90), [0, 1, 0])

    T = trimesh.transformations.translation_matrix([0, 0, 0.0001])
    # mesh.apply_transform(rotation)
    # mesh.apply_transform(rotation_)
    mesh.apply_transform(T)
    origin_point = np.array([0,0.0065,0])
    direction = np.array([0,0,1])
    ray_visualize = trimesh.load_path(np.hstack((origin_point,origin_point + direction/10)).reshape(-1,2,3))
    change_ray_color(ray_visualize, [255, 0, 0])
    finger_left.visual.face_colors = [0, 255, 255]
    scene = trimesh.Scene([mesh,finger_left,ray_visualize])
    # mesh.export('/home/v-wewei/code/two_stage_pointnet/data/grippers/yumi/coarse/new_box_finger.stl')
    scene.show()